/** Bewegende object - moederklasse
 * movingobject.cpp
 *
 *  Created on: Mar 12, 2009
 *      Author: iveb
 * @author Pieter Nys <pieter-nys@hotmail.com>
 * @date    01/03/11
 */

#include "movingObject.h"
#include <math.h>
#include <iostream>
#include <stdio.h>
using namespace std;

/** Zet alles op 0
 *
 * @return void
 */
void MovingObject::init()
{
    pthread_mutex_init(&wpMutex,NULL);

    // Zet 3 waypoints op 0!
    this->setWp(WayPoint());
    this->setWp(WayPoint());
    this->setWp(WayPoint());
}

MovingObject::MovingObject() {
    this->init();
}

MovingObject::MovingObject(string id){
    init();
    this->setID(id);
}

MovingObject::~MovingObject() {
	// TODO Auto-generated destructor stub
}



void MovingObject::setWp(WayPoint wp) {
        // lock mutex, to prevent writing and reading waypoints at the same time
	mutex_lock(&wpMutex);

        wpPrev2_ = wpPrev_;
	wpPrev_ = wp_;
	wp_ = wp;

        // Unlock mutex
        mutex_unlock(&wpMutex);
}

WayPoint MovingObject::getWp() {
        // lock mutex, to prevent writing and reading waypoints at the same time

        mutex_lock(&wpMutex);
        //printf("Zet lock op: done\n");
        // Haal waypoint
	WayPoint wp = wp_;

        // Unlock mutex
        mutex_unlock(&wpMutex);

        return wp;
}

/** Get vorige waypoint
 * 
 * @return WayPoint
 */
WayPoint MovingObject::getWpPrev() {
        // lock mutex, to prevent writing and reading waypoints at the same time
	mutex_lock(&wpMutex);

        // Haal waypoint
	WayPoint wp = wpPrev_;

        // Unlock mutex
        mutex_unlock(&wpMutex);

        return wp;
}


void MovingObject::setWpGoal(WayPoint goal) {
    // lock mutex, to prevent writing and reading waypoints at the same time
    mutex_lock(&wpMutex);

    this->wpGoal = goal;

    // Unlock mutex
    mutex_unlock(&wpMutex);
}

WayPoint MovingObject::getWpGoal(){
    // lock mutex, to prevent writing and reading waypoints at the same time
    mutex_lock(&wpMutex);

    // Haal waypoint
    WayPoint wp = wpGoal;

    // Unlock mutex
    mutex_unlock(&wpMutex);

    return wp;
}

void MovingObject::setID(string id){
    this->id = id;
}

string MovingObject::getID(){
    return this->id;
}

WayPoint MovingObject::getWpNext(double t) {

	//Declaring local variables
	int xdif;
	int ydif;
	double timedif;
	float velocityX;
	float velocityY;
	int goDistanceX;
	int goDistanceY;
	int futureX;
	int futureY;
        float futureTheta;
	WayPoint wpFuture_;
        WayPoint wp;
        WayPoint wpprev;

        wp = this->getWp();
        wpprev = this->getWpPrev();


	//Calculating the distance
	xdif = wp.getX() - wpprev.getX();
	ydif = wp.getY() - wpprev.getY();

        if(xdif == 0 && ydif == 0)
        {
            wpFuture_.setX(wp.getX());
            wpFuture_.setY(wp.getX());
            wpFuture_.setTheta(wp.getX());

            return wpFuture_;
        }
	//Calculating the timedifference
	timedif = wp.getTimeStamp() - wpprev.getTimeStamp();

	//Computing the velocity
	velocityX = xdif / timedif;
	velocityY = ydif / timedif;

	//Distance in a given time
	goDistanceX = velocityX * t;
	goDistanceY = velocityY * t;
	futureX = wp.getX() + goDistanceX;
	futureY = wp.getY() + goDistanceY;

        // Calculate the new theta
        if(xdif == 0)
        {
            futureTheta = (ydif > 0) ? M_PI/2 : -M_PI/2 ;
        }
        else
        {
            futureTheta = atan(ydif/xdif);
        }

	//Set futureX and futureY in a new WayPoint
	wpFuture_.setX(futureX);
	wpFuture_.setY(futureY);
        wpFuture_.setTheta(futureTheta);

	//Return next WayPoint estimate
	return wpFuture_;
}

/** Bereken snijpunt van dit object en een ander
 * 
 * Bereken het snijpunt van dit MvingObject en het meegegeven MovingObject wanneer
 * ze allebei verdergaan zoals ze net zijn gegaan.
 * In de waypoint zal de timestamp staan van wanneer DIT object op het snijpunt
 * zal aankomen.
 *
 * Er wordt NULL teruggegeven als ze niet snijden
 * @param MovingObject Het object waarmee het snijdt
 * @return WayPoint Hun kruispunt
 */
WayPoint MovingObject::getWpIntersection(MovingObject mo2){
    
    // Zie http://www.ahristov.com/tutorial/geometry-games/intersection-lines.html

    int x1,y1,x2,y2,x3,y3,x4,y4;
    int xr,yr;                      //Resultaat

    x1 = this->getWpPrev().getX();
    y1 = this->getWpPrev().getY();

    x2 = this->getWp().getX();
    y2 = this->getWp().getY();

    x3 = mo2.getWpPrev().getX();
    y3 = mo2.getWpPrev().getY();

    x4 = mo2.getWp().getX();
    y4 = mo2.getWp().getY();

    
    int d = (x1-x2)*(y3-y4) - (y1-y2)*(x3-x4);

    // Evenwijdige lijnen
    if (d == 0)
        return WayPoint(-1,-1,0,-1);

    xr = ((x3-x4)*(x1*y2-y1*x2)-(x1-x2)*(x3*y4-y3*x4))/d;
    yr = ((y3-y4)*(x1*y2-y1*x2)-(y1-y2)*(x3*y4-y3*x4))/d;

    return WayPoint(xr,yr,0,0);
}


